from sensor_msgs.msg import LaserScan
from floor_set.msg import Floor
from std_msgs.msg import String
from lib.calcPoints import calcPoints
from lib.getCenters import getCenters
from lib.tool import calcSpeedByAngle
from lib.PID import PIDController
import rospy
import math
import os
import sys
import termios
import tty
import cv2
import json
import time

def calcRealAngle(angle):
    angle = (angle + 100)/360
    angle = angle * 90 - 45
    return angle

# ����·��
TTY_PATH = "/dev/tty"
# ����������Ϊrawģʽ������
STTY_US = "stty raw -echo -F"
# �����ڻָ�Ϊԭʼģʽ������
STTY_DEF = "stty -raw echo -F"

# PID ��ʼ��
kp = -0.74
ki = -0.01
kd = -0.03

pid = PIDController(kp, ki, kd)

STOP = -1
# �Ƿ���յ�����
isDataReceived = 0
# ת��Ƕ�
steeringAngle = 0
# �����ַ�
inputChar = 0
# ��ǰ�ٶ�
currentSpeed = 2100
# ��Ч����
validDistance = 400
# �м�Ƕ�
midAngle = 80
# ���Ƕ�
maxAngle = 160
# P�������ı���ϵ��

# PIֵ
Pi = 3.1415926
isGetInput = False

DBSCAN_CONFIG = {
    "eps": 25,
    "min_samples": 5,
    "debug": False,
    "size": (validDistance, validDistance),
}

headlight=False
def CamCtlCallback(msg) -> None:
    global STOP, headlight
    STOP = time.time()+2000
    headlight = not headlight
    #STOP = time.time()+1000
    print(msg)


def LidarCallback(msg: LaserScan) -> None:
    """
    �״�ص�����

    Args:
        msg: LaserScan��Ϣ
    Returns:
        None
    """
    # start_time = time.time()

    global isDataReceived, steeringAngle, STOP
    
    if time.time() < STOP:
        return
    
    isDataReceived = 1

    foundLeftObstacle = 0
    foundRightObstacle = 0

    data = list(map(lambda x: x * 100, msg.ranges[:1000]))
    points = calcPoints(data)
    # �������ĵ�����
    centers, img = getCenters(points,validDistance, **DBSCAN_CONFIG)

    if DBSCAN_CONFIG["debug"]:
        cv2.imshow("DEBUG", img)
        cv2.waitKey(1)

    closestCenters = [validDistance, validDistance]
    closestDistances = [validDistance, validDistance]
    closestAngles = [0, 0]

    # �����������ĵ�
    for center in centers:
        x = center[0]
        y = center[1]
        distance = math.sqrt(x * x + y * y)
        angle = math.degrees(math.atan2(y, x))
        # ������������Ч��������ڳ���������
        if distance > validDistance or y < 0:
            continue

        # ���Ҳ�
        if x < 0:
            if distance < closestDistances[0]:
                closestCenters[0] = center
                closestDistances[0] = distance
                closestAngles[0] = angle-90
                if closestAngles[0] < 30:
                    continue
                foundLeftObstacle = 1
        # �����
        elif x > 0:
            if distance < closestDistances[1]:
                closestCenters[1] = center
                closestDistances[1] = distance
                closestAngles[1] = 90-angle
                if closestAngles[1] < 30:
                    continue
                foundRightObstacle = 1
    if foundLeftObstacle and foundRightObstacle:
        # ����߼��ұ�Ϊ���
        #print(closestCenters)
        value = abs(closestCenters[1][0]) - abs(closestCenters[0][0])
        steeringAngle = pid.calculate(0, value)
    elif foundLeftObstacle and not foundRightObstacle:
        steeringAngle = 260
        rospy.loginfo("Left MAX\r")
    elif not foundLeftObstacle and foundRightObstacle:
        steeringAngle = -100
        rospy.loginfo("Right MAX\r")
    else:
        steeringAngle = 0
        rospy.loginfo("No obstacle\r")
    # rospy.loginfo(f"{round(time.time() - start_time,2)} Hz\r")


def readChar() -> str:
    """
    ��ȡ��������

    Args:
        None
    Returns:
        char: ��ȡ�����ַ�
    """
    global isGetInput
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        isGetInput = False
    return ch

if __name__ == "__main__":
    try:
        # �����״�����
        rospy.init_node("lidar_node")
        pub = rospy.Publisher("car_vel", Floor, queue_size=2)
        lidar_sub = rospy.Subscriber("/scan", LaserScan, LidarCallback)
        visual_sub = rospy.Subscriber('/visual', String, CamCtlCallback)

        # ��ȡ��������
        while not rospy.is_shutdown():
            # inputChar = readChar()
            if inputChar == "q":
                kp += 0.01
            elif inputChar == "a":
                kp -= 0.01
            elif inputChar == "w":
                ki += 0.01
            elif inputChar == "s":
                ki -= 0.01
            elif inputChar == "e":
                kd += 0.01
            elif inputChar == "d":
                kd -= 0.01
            elif inputChar == "l":
                rospy.loginfo("BYE")
                break

            if isGetInput:
                kp, ki, kd = round(kp, 4), round(ki, 4), round(kd, 4)
                pid = PIDController(kp, ki, kd)
                rospy.loginfo(f"[PID] kp: {kp}\tki: {ki}\tkd: {kd}\r")
                isGetInput = False
            
            if isDataReceived or 1:
                angle = int(midAngle - steeringAngle)
                msg = Floor()
                msg.speed = calcSpeedByAngle(angle, currentSpeed, 50)
                msg.angle = angle
                msg.headlight = 1 if headlight else 0
                pub.publish(msg)
                realAngle = calcRealAngle(steeringAngle)
                rospy.loginfo(
                    f"[{'R' if realAngle > 0 else 'L'}]\Speed: {round(msg.speed,2)}\tangle: {round(realAngle,2)}\t[ {round(kp,4)} {round(ki,4)} {round(kd,4)} ]\r"
                )
                isDataReceived = 0

            rospy.rostime.wallsleep(0.02)
    finally:
        msg = Floor()
        msg.speed = 1500
        msg.angle = midAngle
        msg.horn = 0
        msg.headlight = 0
        msg.stop = 0
        pub.publish(msg)
        os.system(STTY_DEF + " " + TTY_PATH)
